#include "ResultSaver.h"
#include <fstream>
#include <iomanip>
#include <chrono>
#include <filesystem> // For creating directories
#include <iostream> // For error logging

using namespace algo;

void ResultSaver::saveResults(
    const algobase::Paths& initial_paths,
    const std::vector<algobase::FinalPathScore>& optimized_paths,
    const algobase::TaskConfig& config)
{
    std::string timestamp = getCurrentTimestamp();
    std::string result_dir = "result/" + timestamp;

    try {
        std::filesystem::create_directories(result_dir);
    } catch (const std::exception& e) {
        std::cerr << "Error creating result directory: " << e.what() << std::endl;
        return;
    }

    // Save initial paths
    for (size_t i = 0; i < initial_paths.size(); ++i) {
        writePathFile(result_dir + "/initial_path_" + std::to_string(i) + ".txt", initial_paths[i]);
    }

    // Save optimized paths and plans
    for (size_t i = 0; i < optimized_paths.size(); ++i) {
        writeTrajectoryFile(result_dir + "/optimized_path_" + std::to_string(i) + ".txt", optimized_paths[i]);
        writePlanFile(result_dir + "/plan_" + std::to_string(i) + ".txt", optimized_paths[i], config);
    }

    std::cout << "Results saved to directory: " << result_dir << std::endl;
}

std::string ResultSaver::getCurrentTimestamp() {
    auto now = std::chrono::system_clock::now();
    auto in_time_t = std::chrono::system_clock::to_time_t(now);
    std::stringstream ss;
    ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S");
    return ss.str();
}

void ResultSaver::writePathFile(
    const std::string& filepath, 
    const algobase::Path& path)
{
    std::ofstream ofs(filepath);
    if (!ofs) return;
    ofs << "Longitude,Latitude,Depth,Heading,Speed\n";
    for (const auto& p : path) {
        ofs << p.mCoordZ[0] << "," << p.mCoordZ[1] << "," << p.mCoordZ[2] << "," << p.mHeading << "," << p.mSpeed << "\n";
    }
}

void ResultSaver::writeTrajectoryFile(
    const std::string& filepath, 
    const algobase::FinalPathScore& trajectory)
{
    std::ofstream ofs(filepath);
    if (!ofs) return;
    /*ofs << "Longitude,Latitude,Depth,Heading,Speed,Time,"
        <<"TotalScore,L1_Safety,L1_Stealth,L1_Economy,L1_Completion," 
        <<"L2_EnvironmentalNoise,L2_CPCD,L2_AcousticStealth,L2_NavigationDistance,L2_NavigationTime,L2_NavigationEnergy,l2_Motivation,L2_EndPointCompletion,L2_TimeCompletion,L2_MandatoryAreaCompletion,L2_HardRestriction,L2_DangerousAreaExposure\n";
    for (const auto& p : trajectory.TrajectoryCoords) {
        ofs << p.mCoordZ[0] << "," << p.mCoordZ[1] << "," << p.mCoordZ[2] << ","
            << p.mHeading << "," << p.mSpeed << "," << p.mTime << "," << p.mResult << ","
            << p.mL1EvaIndexArray[0] << "," << p.mL1EvaIndexArray[1] << ","
            << p.mL1EvaIndexArray[2] << "," << p.mL1EvaIndexArray[3] << ","
            << p.mL2EvaIndexArray[1] << "," << p.mL2EvaIndexArray[3] << ","
            << p.mL2EvaIndexArray[4] << "," << p.mL2EvaIndexArray[6] << ","
            << p.mL2EvaIndexArray[7] << "," << p.mL2EvaIndexArray[8] << ","
            << p.mL2EvaIndexArray[13] << "," << p.mL2EvaIndexArray[9] << ","
            << p.mL2EvaIndexArray[10] << "," << p.mL2EvaIndexArray[11] << ","
            << p.mL2EvaIndexArray[12] << "," << p.mL2EvaIndexArray[14] << "\n";
    }
    ofs << "Longitude,Latitude,Depth,Heading,Speed,Time,TotalScore,"
        << "L1_Safety,L2_EnvironmentalNoise,L2_CPCD,L2_AcousticStealth,"
        << "L1_Stealth,L2_NavigationDistance,L2_NavigationTime,L2_NavigationEnergy,l2_Motivation,"
        << "L1_Economy,L2_EndPointCompletion,L2_TimeCompletion,L2_MandatoryAreaCompletion,"
        << "L1_Completion,L2_HardRestriction,L2_DangerousAreaExposure\n";
    for (const auto& p : trajectory.TrajectoryCoords) {
        ofs << p.mCoordZ[0] << "," << p.mCoordZ[1] << "," << p.mCoordZ[2] << ","
            << p.mHeading << "," << p.mSpeed << "," << p.mTime << "," << p.mResult << ","
            << p.mL1EvaIndexArray[0] << "," << p.mL2EvaIndexArray[1] << "," << p.mL2EvaIndexArray[3] << "," << p.mL2EvaIndexArray[4] << ","
            << p.mL1EvaIndexArray[1] << "," << p.mL2EvaIndexArray[6] << ","<< p.mL2EvaIndexArray[7] << "," << p.mL2EvaIndexArray[8] << ","<< p.mL2EvaIndexArray[13] << ","
            << p.mL1EvaIndexArray[2] << "," << p.mL2EvaIndexArray[9] << "," << p.mL2EvaIndexArray[10] << "," << p.mL2EvaIndexArray[11] << ","
            << p.mL1EvaIndexArray[3] << "," << p.mL2EvaIndexArray[12] << "," << p.mL2EvaIndexArray[14] << "\n";
    }*/

    ofs << "Score,SafetyScore,StealthScore,EconomyScore,CompletionScore,PathTime,PathDistance,aSpeed,DangerousNums,"
        << "DangerousTime,"
        << "CPCD,"
        << "Smoothness,"
        << "fuelConsumption\n";
    algobase::FinalPathScore p = trajectory; 
    ofs << p.Score << "," << p.SafetyScore << "," << p.StealthScore << ","
        << p.EconomyScore << "," << p.CompletionScore << "," << p.PathTime << "," << p.PathDistance << ","
        << p.aSpeed << "," << p.DangerousNums << "," << p.DangerousTime << "," << p.CPCD << ","
        << p.Smoothness << "," << p.fuelConsumption << "\n";
}

void ResultSaver::writePlanFile(
    const std::string& filepath, 
    const algobase::FinalPathScore& trajectory,
    const algobase::TaskConfig& config)
{
    std::ofstream ofs(filepath);
    if (!ofs) return;

    ofs << "========== Navigation Plan ==========\n\n";
    ofs << "Task Start Time: " << config.mStartTime.to_string() << "\n";
    ofs << "Task End Time:   " << config.mEndTime.to_string() << "\n";
    ofs << "Start Position:  (" << config.mStartPos[0] << ", " << config.mStartPos[1] << ")\n";
    ofs << "End Position:    (" << config.mEndPos[0] << ", " << config.mEndPos[1] << ")\n\n";
    
    ofs << "--- Waypoints ---\n";
    ofs << std::fixed << std::setprecision(6);
    ofs << "ID,Longitude,Latitude,Depth(m),Time(s),TotalScore,L1_Safety,L1_Stealth,L1_Economy,L1_Completion\n";
    for (size_t i = 0; i < trajectory.TrajectoryCoords.size(); ++i) {
        const auto& p = trajectory.TrajectoryCoords[i];
        ofs << i << "," << p.mCoordZ[0] << "," << p.mCoordZ[1] << "," << p.mCoordZ[2] << ","
            << p.mTime << "," << p.mResult << ","
            << p.mL1EvaIndexArray[0] << "," << p.mL1EvaIndexArray[1] << ","
            << p.mL1EvaIndexArray[2] << "," << p.mL1EvaIndexArray[3] << "\n";
    }
}